
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "comm.h"
#include "comm_v1.h"


/**
 * @brief definition of the control block of this comm v1 instance
 */
typedef struct {
    uint32_t parse_step;     /**< step in message frame parsing >*/
    uint32_t content_step;   /**< step in parsing an item that is composed of multiple bytes >*/
    uint32_t current_index;  /**< current parsing index in the data buffer >*/
    uint32_t count;          /**< number of bytes in the buffer >*/
    uint8_t *buffer;         /**< parsed message >*/
    uint32_t buffer_size;    /**< buffer size >*/
    comm_v1_config_t config; /**< configuration >*/
} comm_v1_ctrl_block_t;

/**
 * @brief definition of the message parse state
 */
typedef enum {
    PARSE_STATE_ERROR = 0,  /**< error occurred >*/
    PARSE_STATE_ON_GOING,   /**< more data required to complete the message >*/
    PARSE_STATE_COMPLETE    /**< parse is complete >*/
} parse_state_t;


static inline uint32_t comm_v1_total_length(uint32_t payload_length)
{
 /* COMM V1 messages are in the following structure:
 -------------------------------------------------------------------------------------
 | symbol |  SOF  | Snd | Rcv | SN | ID | LEN | CS0 | D[1] | D[2] | ... | D[N] | CS1 |
 -------------------------------------------------------------------------------------
 |  size  |   1   |  1  |  1  | 1  | 2  |  2  |  1  |  1   |  1   | ... |  1   |  2  |
 -------------------------------------------------------------------------------------
 */
    return sizeof(comm_v1_header_t) + payload_length + 2;
}

static inline uint32_t comm_v1_payload_length(uint32_t total_length)
{
 /* COMM V1 messages are in the following structure:
 -------------------------------------------------------------------------------------
 | symbol |  SOF  | Snd | Rcv | SN | ID | LEN | CS0 | D[1] | D[2] | ... | D[N] | CS1 |
 -------------------------------------------------------------------------------------
 |  size  |   1   |  1  |  1  | 1  | 2  |  2  |  1  |  1   |  1   | ... |  1   |  2  |
 -------------------------------------------------------------------------------------
 */
    return total_length - sizeof(comm_v1_header_t) - 2;
}

static uint32_t is_config_valid(const comm_v1_config_t *config)
{
    if (NULL == config) {
        return 0;
    }

    if (NULL == config->name) {
        return 0;
    }

    return 1;
}

static void distribute_message(comm_v1_ctrl_block_t *ctrl_block, uint8_t *message, uint32_t length)
{
    comm_v1_header_t *head = (comm_v1_header_t *)message;
    if (NULL != ctrl_block->config.message_handler) {
        comm_v1_runtime_t runtime;
        runtime.name = ctrl_block->config.name;
        runtime.id = ctrl_block->config.id;
        runtime.line = ctrl_block->config.line;
        runtime.handle = (comm_v1_handle_t)ctrl_block;
        ctrl_block->config.message_handler(&runtime, head,
                                           &message[sizeof(comm_v1_header_t)],
                                           comm_v1_payload_length(length));
    }
}

static uint16_t comm_v1_make_checksum(uint8_t *data, uint32_t length)
{
    /* a simple implementation: checksum = summary of all bytes */
    uint16_t checksum = 0;
    for (uint32_t index = 0; index < length; index++) {
        checksum += data[index];
    }
    return checksum;
}

static void comm_v1_parse_complete(comm_v1_ctrl_block_t *ctrl_block, uint32_t index_offset)
{
    // update index
    ctrl_block->current_index += index_offset;

    // fire for the message process action
    distribute_message(ctrl_block, ctrl_block->buffer, ctrl_block->current_index);

    // reset the parse steps
    ctrl_block->parse_step = 0;
    ctrl_block->content_step = 0;

    // move the left data in buffer to pop out the processed bytes
    for (uint32_t i = ctrl_block->current_index; i < ctrl_block->count; i++) {
        ctrl_block->buffer[i - ctrl_block->current_index] = ctrl_block->buffer[i];
    }
    ctrl_block->count -= ctrl_block->current_index;
    ctrl_block->current_index = 0;
}

static void comm_v1_parse_error(comm_v1_ctrl_block_t *ctrl_block, uint32_t index_offset, parse_error_type_t error_type)
{
    // reset the parse steps
    ctrl_block->parse_step = 0;
    ctrl_block->content_step = 0;

    // pop out one byte and start over again
    for (uint32_t i = 0; i < ctrl_block->count - 1; i++) {
        ctrl_block->buffer[i] = ctrl_block->buffer[i + 1];
    }
    ctrl_block->count -= 1;
    ctrl_block->current_index = 0;

    // inform error type
    if (NULL != ctrl_block->config.parse_error_handler) {
        comm_v1_runtime_t runtime;
        runtime.name = ctrl_block->config.name;
        runtime.id = ctrl_block->config.id;
        runtime.line = ctrl_block->config.line;
        runtime.handle = (comm_v1_handle_t)ctrl_block;
        ctrl_block->config.parse_error_handler(&runtime, error_type);
    }
}

static void comm_v1_parse_on_going(comm_v1_ctrl_block_t *ctrl_block, uint32_t index_offset)
{
    // update index
    ctrl_block->current_index += index_offset;
}

static void comm_v1_append_checksum1(uint8_t *buffer, uint32_t offset, uint16_t checksum1)
{
    buffer[offset] = checksum1 & 0xFF;
    buffer[offset + 1] = (checksum1 >> 8) & 0xFF;
}

comm_v1_handle_t comm_v1_create(const comm_v1_config_t *config)
{
    if (!is_config_valid(config)) {
        return NULL;
    }

    uint32_t total_length = sizeof(comm_v1_ctrl_block_t) + comm_v1_total_length(config->max_message_length);
    uint8_t *memory_pool = (uint8_t *)malloc(total_length);
    if (NULL == memory_pool) {
        return NULL;
    }

    comm_v1_ctrl_block_t *ctrl_block = (comm_v1_ctrl_block_t *)memory_pool;
    ctrl_block->parse_step = 0;
    ctrl_block->content_step = 0;
    ctrl_block->current_index = 0;
    ctrl_block->count = 0;
    ctrl_block->buffer = (uint8_t *)&memory_pool[sizeof(comm_v1_ctrl_block_t)];
    ctrl_block->buffer_size = comm_v1_total_length(config->max_message_length);
    memcpy(&ctrl_block->config, config, sizeof(comm_v1_config_t));

    return (comm_v1_handle_t)ctrl_block;
}

void comm_v1_reset(comm_v1_handle_t handle)
{
    if (handle != NULL)
    {
        comm_v1_ctrl_block_t *ctrl_block = (comm_v1_ctrl_block_t *)handle;
        ctrl_block->parse_step = 0;
        ctrl_block->content_step = 0;
        ctrl_block->current_index = 0;
        ctrl_block->count = 0;
    }
}

int32_t comm_v1_send_message(comm_v1_handle_t handle, uint8_t sender, uint8_t receiver,
                             uint8_t seq_num, uint16_t message_id, const uint8_t *payload, uint32_t length)
{
    if (NULL == handle ) {
        // printf("%s,%d is error\r\n",__func__,__LINE__);
        return FAIL;
    }
    if((NULL == payload && length > 0)) {
        // printf("%s,%d is error\r\n",__func__,__LINE__);
        return FAIL;
    }

    comm_v1_ctrl_block_t *ctrl_block = (comm_v1_ctrl_block_t *)handle;
    uint32_t total_length = comm_v1_total_length(length);
    uint8_t *buffer = (uint8_t *)malloc(total_length);
    if (NULL == buffer) {
        return FAIL;
    }
    
    // build message frame
    comm_v1_header_t *head = (comm_v1_header_t *)buffer;
    head->start = ctrl_block->config.start_of_frame;
    head->sender = sender;
    head->receiver = receiver;
    head->seq_num = seq_num;
    head->message_id = message_id;
    head->length = length;
    head->checksum0 = (uint8_t)comm_v1_make_checksum(buffer, sizeof(comm_v1_header_t) - 1);
    memcpy(&buffer[sizeof(comm_v1_header_t)], payload, length);
    uint16_t checksum1 = comm_v1_make_checksum(buffer, sizeof(comm_v1_header_t) + length);
    comm_v1_append_checksum1(buffer, sizeof(comm_v1_header_t) + length, checksum1);

    // send the message out
    comm_v1_runtime_t runtime;
    runtime.name = ctrl_block->config.name;
    runtime.id = ctrl_block->config.id;
    runtime.line = ctrl_block->config.line;
    runtime.handle = (comm_v1_handle_t)ctrl_block;
    uint32_t sent_count = 0;
    while (sent_count < total_length) {
        uint32_t nbyte = ctrl_block->config.transmit_handler(&runtime, &buffer[sent_count], total_length - sent_count);
        if (0 == nbyte) {
            free(buffer);
            return FAIL;
        }
        sent_count += nbyte;
    }

    free(buffer);
    return SUCC;
}

static parse_state_t comm_v1_parse_message(comm_v1_ctrl_block_t *ctrl_block,
                                           uint8_t *data, uint32_t length,
                                           uint32_t *index, parse_error_type_t *error_type)
{
    uint32_t loop = 0;
    parse_state_t state = PARSE_STATE_ON_GOING;

    do {
        switch (ctrl_block->parse_step)
        {
            case 0 : {
                /* start */
                if (ctrl_block->config.start_of_frame == data[loop++]) {
                    ctrl_block->parse_step++;
                    ctrl_block->content_step = 0;
                } else {
                    state = PARSE_STATE_ERROR;
                    *error_type = PARSE_ERROR_TYPE_SOF;
                }
                break;
            }

            case 1 : {
                /* sender */
                loop++;
                ctrl_block->parse_step++;
                break;
            }

            case 2 : {
                /* receiver */
                loop++;
                ctrl_block->parse_step++;
                break;
            }

            case 3 : {
                /* seq_num */
                loop++;
                ctrl_block->parse_step++;
                break;
            }

            case 4 : {
                /* msg_id */
                loop++;
                ctrl_block->content_step++;
                if (2 == ctrl_block->content_step) {
                    ctrl_block->content_step = 0;
                    ctrl_block->parse_step++;
                }
                break;
            }

            case 5 : {
                /* length */
                loop++;
                ctrl_block->content_step++;
                if (2 == ctrl_block->content_step) {
                    ctrl_block->content_step = 0;
                    comm_v1_header_t *head = (comm_v1_header_t *)ctrl_block->buffer;
                    if (head->length > ctrl_block->config.max_message_length) {
                        state = PARSE_STATE_ERROR;
                        *error_type = PARSE_ERROR_TYPE_LENGTH;
                    } else {
                        ctrl_block->parse_step++;
                    }
                }
                break;
            }

            case 6 : {
                /* checksum0 */
                if (data[loop++] == (uint8_t)comm_v1_make_checksum(ctrl_block->buffer, sizeof(comm_v1_header_t) - 1)) {
                    comm_v1_header_t *head = (comm_v1_header_t *)ctrl_block->buffer;
                    if (head->length > 0) {
                        ctrl_block->parse_step++;
                    } else {
                        // support message without payload
                        ctrl_block->parse_step += 2;
                    }
                } else {
                    state = PARSE_STATE_ERROR;
                    *error_type = PARSE_ERROR_TYPE_CHECKSUM0;
                }
                break;
            }

            case 7 : {
                /* message body */
                loop++;
                ctrl_block->content_step++;
                comm_v1_header_t *head = (comm_v1_header_t *)ctrl_block->buffer;
                if (ctrl_block->content_step >= head->length) {
                    ctrl_block->content_step = 0;
                    ctrl_block->parse_step++;
                }
                break;
            }

            case 8 : {
                /* checksum1 */
                loop++;
                ctrl_block->content_step++;
                if (2 == ctrl_block->content_step) {
                    comm_v1_header_t *head = (comm_v1_header_t *)ctrl_block->buffer;
                    uint32_t total_length = comm_v1_total_length(head->length);
                    uint32_t offset = total_length - 2;
                    uint16_t checksum1 = (uint16_t)ctrl_block->buffer[offset] | (uint16_t)(ctrl_block->buffer[offset + 1] << 8);

                    if (checksum1 == comm_v1_make_checksum(ctrl_block->buffer, total_length - 2)) {
                        state = PARSE_STATE_COMPLETE;
                    } else {
                        state = PARSE_STATE_ERROR;
                        *error_type = PARSE_ERROR_TYPE_CHECKSUM1;
                    }
                }
                break;
            }

            default:
                state = PARSE_STATE_ERROR;
                *error_type = PARSE_ERROR_TYPE_UNDEFINED;
                break;
        }
    } while ((loop < length) && (PARSE_STATE_ON_GOING == state));

    *index = loop;
    return state;
}


int32_t comm_v1_process(comm_v1_handle_t handle, const uint8_t *data, uint32_t length)
{
    if (NULL == handle || NULL == data) {
        return FAIL;
    }

    comm_v1_ctrl_block_t *ctrl_block = (comm_v1_ctrl_block_t *)handle;

    uint32_t remain_length = length;
    while (remain_length > 0) {
        // copy some of the data into the buffer
        uint32_t copy_length = MIN(ctrl_block->buffer_size - ctrl_block->count, remain_length);
        memcpy(&ctrl_block->buffer[ctrl_block->count], &data[length - remain_length], copy_length);
        ctrl_block->count += copy_length;
        remain_length -= copy_length;

        while (ctrl_block->current_index < ctrl_block->count) {
            // parse message frames with the data in buffer
            uint32_t index_offset = 0;
            parse_error_type_t error_type;
            parse_state_t state = comm_v1_parse_message(ctrl_block,
                                                        &ctrl_block->buffer[ctrl_block->current_index],
                                                        ctrl_block->count - ctrl_block->current_index,
                                                        &index_offset,
                                                        &error_type);
            if (PARSE_STATE_COMPLETE == state) {
                comm_v1_parse_complete(ctrl_block, index_offset);
            } else if (PARSE_STATE_ERROR == state) {
                comm_v1_parse_error(ctrl_block, index_offset, error_type);
            } else {
                comm_v1_parse_on_going(ctrl_block, index_offset);
            }
        }
    }

    return SUCC;
}

int32_t comm_v1_once_parse(comm_v1_handle_t handle, const uint8_t *data, uint16_t data_len,comm_v1_header_t *header,uint8_t *parse_msg,uint16_t parse_max_len)
{
    if (NULL == handle || NULL == data || NULL == parse_msg || 
        NULL == header || data_len == 0 || parse_max_len==0) {
        return -1;
    }

    comm_v1_ctrl_block_t *ctrl_block = (comm_v1_ctrl_block_t *)handle;
    uint32_t remain_length = data_len;
    while (remain_length > 0) {
        // copy some of the data into the buffer
        uint32_t copy_length = MIN(ctrl_block->buffer_size - ctrl_block->count, remain_length);
        memcpy(&ctrl_block->buffer[ctrl_block->count], &data[data_len - remain_length], copy_length);
        ctrl_block->count += copy_length;
        remain_length -= copy_length;

        while (ctrl_block->current_index < ctrl_block->count) {
            // parse message frames with the data in buffer
            uint32_t index_offset = 0;
            parse_error_type_t error_type;
            parse_state_t state = comm_v1_parse_message(ctrl_block,
                                                        &ctrl_block->buffer[ctrl_block->current_index],
                                                        ctrl_block->count - ctrl_block->current_index,
                                                        &index_offset,
                                                        &error_type);
            if (PARSE_STATE_COMPLETE == state) {
                ctrl_block->current_index += index_offset;
                int32_t parse_data_len =comm_v1_payload_length(ctrl_block->current_index);
                if(parse_data_len > parse_max_len) {
                    return (-parse_data_len);
                }
                memcpy(header,ctrl_block->buffer, sizeof(comm_v1_header_t));
                memcpy(parse_msg,&ctrl_block->buffer[sizeof(comm_v1_header_t)],parse_data_len);
                // reset the parse steps
                ctrl_block->parse_step = 0;
                ctrl_block->content_step = 0;

                // move the left data in buffer to pop out the processed bytes
                for (uint32_t i = ctrl_block->current_index; i < ctrl_block->count; i++) {
                    ctrl_block->buffer[i - ctrl_block->current_index] = ctrl_block->buffer[i];
                }

                ctrl_block->count -= ctrl_block->current_index;
                ctrl_block->current_index = 0;
                // printf("parse ok len:%d\r\n",parse_data_len);
                return  parse_data_len; 
            } else if (PARSE_STATE_ERROR == state) {
                // printf("parse error\r\n");
                comm_v1_parse_error(ctrl_block, index_offset, error_type);
            } else {
                // printf("going\r\n");
                comm_v1_parse_on_going(ctrl_block, index_offset);
            }
        }
    }

    return 0;
}

// int32_t comm_v1_once_parse(comm_v1_handle_t handle, const uint8_t *data, uint16_t data_len,comm_v1_header_t *header,uint8_t *parse_msg,uint16_t parse_len)
// {
//     if (NULL == handle || NULL == data || NULL == parse_msg || 
//         NULL == header || data_len == 0 || parse_len==0) {
//         return -1;
//     }

//     comm_v1_ctrl_block_t *ctrl_block = (comm_v1_ctrl_block_t *)handle;
//     uint32_t remain_length = data_len;
//     while (remain_length > 0) {
//         // copy some of the data into the buffer
//         uint32_t copy_length = MIN(ctrl_block->buffer_size - ctrl_block->count, remain_length);
//         memcpy(&ctrl_block->buffer[ctrl_block->count], &data[data_len - remain_length], copy_length);
//         ctrl_block->count += copy_length;
//         remain_length -= copy_length;

//         while (ctrl_block->current_index < ctrl_block->count) {
//             // parse message frames with the data in buffer
//             uint32_t index_offset = 0;
//             parse_error_type_t error_type;
//             parse_state_t state = comm_v1_parse_message(ctrl_block,
//                                                         &ctrl_block->buffer[ctrl_block->current_index],
//                                                         ctrl_block->count - ctrl_block->current_index,
//                                                         &index_offset,
//                                                         &error_type);
//             if (PARSE_STATE_COMPLETE == state) {
//                 ctrl_block->current_index += index_offset;
//                 int32_t parse_data_len =comm_v1_payload_length(ctrl_block->current_index);
//                 if(parse_data_len == parse_len) {
//                     memcpy(header,ctrl_block->buffer, sizeof(comm_v1_header_t));
//                     memcpy(parse_msg,&ctrl_block->buffer[sizeof(comm_v1_header_t)],parse_data_len);
//                 } else if(parse_data_len > parse_len) {
//                     // return -2;/* overflow */
//                     return 0;/* overflow */
//                 } else {
//                     // return -3; /* not need */
//                     return 0; /* not need */
//                 }
//                 // reset the parse steps
//                 ctrl_block->parse_step = 0;
//                 ctrl_block->content_step = 0;

//                 // move the left data in buffer to pop out the processed bytes
//                 for (uint32_t i = ctrl_block->current_index; i < ctrl_block->count; i++) {
//                     ctrl_block->buffer[i - ctrl_block->current_index] = ctrl_block->buffer[i];
//                 }

//                 ctrl_block->count -= ctrl_block->current_index;
//                 ctrl_block->current_index = 0;
//                 printf("parse ok len:%d\r\n",parse_data_len);
//                 return  parse_data_len; 
//             } else if (PARSE_STATE_ERROR == state) {
//                 printf("parse error\r\n");
//                 comm_v1_parse_error(ctrl_block, index_offset, error_type);
//             } else {
//                 // printf("going\r\n");
//                 comm_v1_parse_on_going(ctrl_block, index_offset);
//             }
//         }
//     }

//     return 0;
// }

int32_t comm_v1_destroy(comm_v1_handle_t handle)
{
    if (NULL == handle) {
        return FAIL;
    }

    comm_v1_ctrl_block_t *ctrl_block = (comm_v1_ctrl_block_t *)handle;
    free(ctrl_block);

    return SUCC;
}
